/*
 * @Date: 2020-10-03 20:58:03
 * @LastEditors: Tonser
 * @LastEditTime: 2020-10-03 23:11:51
 * @FilePath: /swarm_ws/src/px4_swarm_control/include/SwarmMessage.h
 * @Description: 用于Swarm 基础 data 库
 */

#ifndef SRC_SWARMMESSAGE_H
#define SRC_SWARMMESSAGE_H

#include <vector>
#include "../Utils/Serial_Utils.h"

namespace msgType
{
    enum COMMAND : unsigned char
    { // 预留 0x00-0x0f
        CMD_POS_ENABLE = 0x00,
        CMD_ATT_ENABLE = 0x01,
        CMD_ALT_ENABLE = 0x02,
        CMD_EMERGENC_ENABLE = 0x03,
        CMD_HOVER = 0x04,
        CMD_LAND = 0x05,
        CMD_RETURN_HOME = 0x06,
        CMD_START_MISSION = 0x07,
        CMD_STOP_MISSION = 0x08
    };

    enum STATUS : unsigned char
    { // 预留 0x10-0x1f
        STS_POS = 0x10,
        STS_ATT = 0x11,
        STS_VEL = 0x12
    };

    enum SETPOINT : unsigned char
    { // 预留 0x20-0x2f
        SEP_POS = 0x20,
        SEP_ATT = 0x21,
        SEP_VEL = 0x22
    };

    enum SWARM_STATUS : unsigned char
    {
        SWM_STS_POS = 0x30,
        SWM_STS_ATT = 0x31,
        SWM_STS_VEL = 0x32
    };

    enum DETECT_STATUS : unsigned char
    {
        DETECT_STS_POS = 0x40
    };
} // namespace msgType

DATA_FRAM CMD_ENABLE_FRAME
{
    unsigned char head[2];
    unsigned char msg_type;
    unsigned char value;
    unsigned char armed;
    unsigned char chk;
    unsigned char end;
};

DATA_FRAM DRONE_STATUS3_FRAME
{
    unsigned char head[2];
    unsigned char msg_type;
    unsigned char id;
    int data[3];
    unsigned char chk;
    unsigned char end;
};

DATA_FRAM DRONE_STATUS4_FRAME
{
    unsigned char head[2];
    unsigned char msg_type;
    unsigned char id;
    int data[4];
    unsigned char chk;
    unsigned char end;
};

class SwarmMessageUtils{

    static int
    getMsgLength(unsigned char msg_type)
    {
        int length = -1;
        switch (msg_type)
        {
        case msgType::CMD_ALT_ENABLE:
        case msgType::CMD_ATT_ENABLE:
        case msgType::CMD_EMERGENC_ENABLE:
        case msgType::CMD_HOVER:
        case msgType::CMD_LAND:
        case msgType::CMD_POS_ENABLE:
        case msgType::CMD_RETURN_HOME:
        case msgType::CMD_START_MISSION:
        case msgType::CMD_STOP_MISSION:
            length = sizeof(CMD_ENABLE_FRAME);
            break;
        case msgType::STS_POS:
        case msgType::STS_VEL:
        case msgType::SEP_POS:
        case msgType::SEP_VEL:
        case msgType::SWM_STS_POS:
        case msgType::SWM_STS_VEL:
        case msgType::DETECT_STS_POS:
            length = sizeof(DRONE_STATUS3_FRAME);
            break;
        case msgType::STS_ATT:
        case msgType::SEP_ATT:
        case msgType::SWM_STS_ATT:
            length = sizeof(DRONE_STATUS4_FRAME);
            break;
        default:
            length = -1;
            break;
        }
        return length;
    }


    static void
    getMsgHead(Buffer &data, unsigned char type)
    {
        data.clear();
        data.push_back(0xFE);
        data.push_back(0x22);
        data.push_back(type);
    }

    /**
     * @description: 根据现有消息返回添加了校验字节的消息体
     * @param <消息类型> 装好内容的消息体
     * @return 添加了校验字节的消息体
     */
    template<typename SwarmMessage>
    static Buffer
    getCheckedBuffer(SwarmMessage &msg){
        unsigned char chk = 0x00;
        Buffer data;
        unsigned char size = sizeof(SwarmMessage);
        data.resize(size);
        memcpy(&data[0],&msg,size);
        for (int i = 0; i < size-2; i++)
        {
            chk += data[i];
        }
        data[size-2] = chk;
        msg.chk = chk;
        return data;
    }
};


#endif //SRC_SWARMMESSAGE_H
